#include "KalmanFilter.h"
namespace hitcrt {
void KalmanFilter::init(const std::vector<boost::any> &param) {
    // 初始化三个滤波器 暂时使用一个参数
    // 遍历 XYZ KFC滤波器 初始化
    std::vector<boost::any> param1;
    std::vector<double> processCov;
    std::vector<double> measureCov;
    processCov.emplace_back(0.1);
    measureCov.emplace_back(0.1);
    param1.push_back(processCov);
    param1.push_back(measureCov);
    for (int i = 0; i < 3; i++) {
        m_filter[i].init(param1);
    }
}

void KalmanFilter::updated(cv::Point3f lastPoint, cv::Point3f newPoint,
                           const std::vector<boost::any> &param) {
    // 时间
    m_deltaTime = boost::any_cast<double>(param[0]);
    // 测量误差
    const std::vector<double> measureCov =
        boost::any_cast<std::vector<double>>(param[1]);

    // 测量数据 XYZ顺序
    const std::vector<double> measure =
        boost::any_cast<std::vector<double>>(param[2]);

    cv::Point3f pointCalculateV;  // 用来计算速度的过度点
    pointCalculateV = (newPoint - lastPoint) / m_deltaTime;
    std::vector<double> measureV;
    measureV.emplace_back(pointCalculateV.x);
    measureV.emplace_back(pointCalculateV.y);
    measureV.emplace_back(pointCalculateV.z);

    for (int i = 0; i < m_filter.size(); i++) {
        std::vector<boost::any> paramKFC;

        paramKFC.push_back(m_deltaTime);  // int
        paramKFC.push_back(measureCov);

        std::vector<double> measureXYZ;
        measureXYZ.emplace_back(measure[i]);
        measureXYZ.emplace_back(measureV[i]);
        paramKFC.push_back(measureXYZ);

        m_filter[i].setParam(paramKFC);
        m_filter[i].predict();
    }
    //getResult();
}

void KalmanFilter::getResult(cv::Point3f &resultKfilter) {
    // for (int i = 0; i < m_filter.size(); i++) {
    //     std::cout << "第" << i << "结果：" << m_filter[i].getResult()
    //               << std::endl;
    // }
    resultKfilter.x = m_filter[0].getResult().at<double>(0, 0);
    resultKfilter.y = m_filter[1].getResult().at<double>(0, 0);
    resultKfilter.z = m_filter[2].getResult().at<double>(0, 0);
}

}  // namespace hitcrt